%% 获取A*算法搜索代价表
function costmap = GridAStar(cfg,veh,goal,animation)
    % 获取参数
    obstList = cfg.OBSTACLE_LIST;
    minx = cfg.MINX;
    maxx = cfg.MAXX;
    miny = cfg.MINY;
    maxy = cfg.MAXY;
    gridResolution = cfg.XY_GRID_RESOLUTION;
    W = veh.W;
    LF = veh.LF;
    LB = veh.LB;

    % 障碍物膨胀距离
    expandDist = (sqrt(W^2+(LF+LB)^2)/2)/gridResolution;  % 车辆斜对角距离的一半
    expandDist = expandDist - 1/gridResolution;  % 目标容忍度范围扩展

    % 初始化代价表
    obmap = CalcObstMap(obstList,minx,maxx,miny,maxy,gridResolution,expandDist);

    % 初始化目标点
    x = goal(1);
    y = goal(2);
    x = ceil((x-minx)/gridResolution);
    y = ceil((y-miny)/gridResolution);
    goal = [x,y];
    costmap = 0*obmap;
    dim = size(obmap);

    % 计算代价表
    for i = 1:dim(1)
        for j = 1:dim(2)
            if obmap(i,j) == 1
                costmap(i,j) = inf;
                continue
            elseif i == x && j == y
                continue
            end
            start = [i,j];
            cost = AStarSearch(start,goal,obmap);
            costmap(i,j) = cost;
        end
    end

    % 热力图可视化
    if animation
        % 逆时针旋转90度显示
        costmap1 = costmap';
        costmap1 = flipud(costmap1);
        imagesc(costmap1);
    end
end

% 根据障碍物列表和网格分辨率计算代价表
function obmap = CalcObstMap(obstList,minx,maxx,miny,maxy,gridResolution,expandDist)
    xwidth = maxx - minx;
    xwidth = ceil(xwidth/gridResolution);
    ywidth = maxy - miny;
    ywidth = ceil(ywidth/gridResolution);
    obmap = zeros(ywidth,xwidth);
    for i = 1:xwidth
        for j = 1:ywidth
            ix = minx+(i-1/2)*gridResolution;
            iy = miny+(j-1/2)*gridResolution;
            [~,D] = knnsearch(obstList,[ix,iy]);
            if D < expandDist
                obmap(i,j) = 1;
            end
        end
    end
end

% A*搜索算法搜索代价
function cost = AStarSearch(start,goal,obmap)
    dim = size(obmap);
    % Grids(i,j,1) - x of parent pos; 2 - y of parent pos; 3 - precost; 4 -
    % postcost
    Grids = zeros(dim(1),dim(2),4);
    for i = 1:dim(1)
        for j = 1:dim(2)
            Grids(i,j,1) = i; % 父节点的所在行
            Grids(i,j,2) = j; % 父节点的所在列
            Grids(i,j,3) = norm(([i,j]-goal)); % 启发值h
            Grids(i,j,4) = inf; % g值
        end
    end
    Open = start;
    Grids(start(1),start(2),4) = 0;
    Close = [];
    while ~isempty(Open)
        [wknode,Open] = PopOpen(Open,Grids);
        [Grids,Open,Close] = Update(wknode,goal,obmap,Grids,Open,Close);
        Close(end+1,:) = wknode; %#ok<*AGROW>
    end
    cost = Grids(goal(1),goal(2),3)+Grids(goal(1),goal(2),4);
end

% 更新当前节点周围节点的状态（是否在 Open 或 Close 列表中，以及它们的成本）
function [Grids,Open,Close] = Update(wknode,goal,obmap,Grids,Open,Close)
    dim = size(obmap);
    for i = -1:1
        for j = -1:1
            adjnode = wknode+[i,j];
            row = adjnode(1);
            col = adjnode(2);
            if i == 0 && j == 0
                continue
            elseif row < 1 || row > dim(1)
                continue
            elseif col < 1 || col > dim(2)
                continue
            elseif obmap(row,col) == 1
                continue
            end
            tcost = Grids(wknode(1),wknode(2),4)+norm([i,j]);
            if Grids(row,col,4) > tcost
                Grids(row,col,1) = wknode(1);
                Grids(row,col,2) = wknode(2);
                Grids(row,col,4) = tcost;
                % add adjnode to Open except wknode is goal
                if ~ismember(adjnode,Open,'rows') && ~isequal(adjnode,goal)
                    Open(end+1,:) = adjnode;
                end
                % if adjnode is in Close remove it
                if isempty(Close)
                    % do nothing
                elseif ismember(adjnode,Close,'rows')
                    [~,rIdx] = ismember(adjnode,Close,'rows');
                    Close(rIdx,:) = [];
                end
            end
        end
    end
end

% 从 Open 列表中取出具有最小 f 值的节点（f = g + h，其中 g 是从起点到当前节点的实际成本，h 是启发式估计成本到目标点）
function [wknode,Open] = PopOpen(Open,Grids)
    mincost = inf;
    minidx = 1;
    for i = 1:size(Open,1)
        node = Open(i,:);
        tcost = Grids(node(1),node(2),3)+Grids(node(1),node(2),4);
        if tcost < mincost
            minidx = i;
            mincost = tcost;
        end
    end
    wknode = Open(minidx,:);
    Open(minidx,:) = [];
end